/////////////////////////
// File: camera_array_main.cpp
// Authors: Brian Fehrman and Scott Logan and Matt Richard
// Date: 3-27-2013
// 
// Purpose: Implementation file for the camera_array_main function.
//
/////////////////////////

///////// Includes ////////////
#include "camera_array_main.h"

int main( int argc, char *argv[])
{  
   string settings_file = "xml/camera_array_settings.xml";
   
   if( argc > 1 )
      settings_file = argv[ 1 ];
   
   FileStorage fs( settings_file, FileStorage::READ);
   
   if( ! fs.isOpened() )
   {
      cout << "Error opening settings file: " << settings_file << endl;
      return -1;
   }
   
   int run_init_cams;
   int run_capture;
   int run_calibration;
   
   fs["basic_settings"]["run_init_cams"] >> run_init_cams;
   fs["basic_settings"]["run_capture"] >> run_capture;
   fs["basic_settings"]["run_calibration"] >> run_calibration;
   
   if( run_init_cams )
   {
      cout << "**********************************" << endl;
      cout << "INITIALIZING CAMERA SETTINGS" << endl << endl;
      init_cams( fs["cam_settings"] );
      cout << endl << "DONE INITIALIZING CAMERA SETTINGS" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_capture )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING IMAGE CAPTURE" << endl << endl;
      capture_images( fs["cap_settings"] );
      cout << endl << "DONE RUNNING IMAGE CAPTURE" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_calibration )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING CAMERA CALIBRATION" << endl << endl;
      calibrate( fs["calib_settings"] );
      cout << endl << "DONE RUNNING CAMERA CALIBRATION" << endl;
      cout << "**********************************" << endl;
   }
   
   //test_display();
   
   template_matching_four_camera();
   
   Mat image_1, image_2, matches, diff_mat;
   
   image_1 = imread( "images_new_array_16_sub/cam_0_0/31_gray_320_240.jpg" );
   image_2 = imread( "images_new_array_16_sub/cam_0_1/31_gray_320_240.jpg" );
   
   Mat gray_1, gray_2;
   
   cvtColor( image_1, gray_1, CV_BGR2GRAY );
   cvtColor( image_2, gray_2, CV_BGR2GRAY );
   
   gray_1.convertTo( gray_1, CV_32F, 1/255.0 );
   gray_2.convertTo( gray_2, CV_32F, 1/255.0 );
   
   Mat temp_gray_1 = gray_2.rowRange( 0, 20 ).colRange( 10, 60 );
   Mat temp_gray_2 = gray_2.rowRange( 0, 30 ).colRange( 0, 100 );
   
   Point max_loc;
   
  // my_phase_correlation( temp_gray_2, temp_gray_1, max_loc );
   
   //cout << "Mine: " << max_loc << endl;
   
cout << phaseCorrelate( gray_1, gray_2 );
  //template_matching( gray_2, gray_1, 15, Point( 100, 15) , matches);
  // compute_diff_mat(  gray_2, matches, diff_mat, true, false );
   
   //cout << "Now" << endl;
   
   
   namedWindow( "yup", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
   namedWindow( "in_color", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
    namedWindow( "res_1", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
     namedWindow( "res_2", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
   
   Mat diff_mat_color( diff_mat.rows, diff_mat.cols + 50, CV_8UC3 );
   
   //cvtColor( diff_mat, diff_mat_color, CV_GRAY2RGB );
   //cvtColor( diff_mat_color, diff_mat_color, CV_RGB2HSV );
   
   
   for( int row = 0; row < diff_mat.rows; row++ )
   {
      for( int col = 0; col < diff_mat.cols; col++ )
      {
         diff_mat_color.at< Vec3b >( row, col )[ 0 ] = diff_mat.at< unsigned char >( row, col ) * 1.5;
         diff_mat_color.at< Vec3b >( row, col )[ 1 ] = 255;
         diff_mat_color.at< Vec3b >( row, col )[ 2 ] = 255;
      }
   }
   
   for( int row =  0; row < diff_mat.rows; row++ )
   {
      for( int col = diff_mat.cols; col < diff_mat.cols + 50; col++ )
      {
         diff_mat_color.at< Vec3b >( row, col )[ 0 ] =  ( float ) row / diff_mat.rows * 150;
         diff_mat_color.at< Vec3b >( row, col )[ 1 ] = 255;
         diff_mat_color.at< Vec3b >( row, col )[ 2 ] = 255;
      }
   }
   
   cvtColor( diff_mat_color, diff_mat_color, CV_HSV2RGB );
   //cvtColor( image_2, image_2, CV_GRAY2RGB );
   //cvtColor( image_1, image_1, CV_GRAY2RGB );
   
  // imwrite( "r_image.jpg", image_2 );
  // imwrite( "l_image.jpg", image_1 );
  // imwrite( "diff_map_new_comp_gauss_3x3_1_90_95_5_im.jpg", diff_mat_color );
   
   for( ;; )
   {
      imshow( "in_color", diff_mat_color );
      imshow( "yup", diff_mat );
      imshow( "res_1", image_1 );
      imshow( "res_2", image_2 );
      
      if( waitKey( 30 ) > 0 ) break;
   }
   
   fs.release();
   
   return 0;
}




